#include "SliceContoursExtractor.h"

// 判断两个向量是否相等，考虑浮点数误差
bool isEqual(const QVector3D& a, const QVector3D& b) {
    const float epsilon = std::numeric_limits<float>::epsilon();
    return std::fabs(a.x() - b.x()) < epsilon &&
           std::fabs(a.y() - b.y()) < epsilon &&
           std::fabs(a.z() - b.z()) < epsilon;
}

// 去重函数
void removeDuplicates(QVector<QVector3D>& points, float epsilon = 1e-4f) {
    QVector<QVector3D> unique;
    for (const QVector3D& p : points) {
        bool isDuplicate = false;
        for (const QVector3D& up : unique) {
            if ((p - up).lengthSquared() < epsilon * epsilon) {
                isDuplicate = true;
                break;
            }
        }
        if (!isDuplicate) {
            unique.append(p);
        }
    }
    points = unique;
}

void g0(const Mesh& in_mesh, const QVector3D& in_plane_norm, float in_lambda, Edge& out_edge)
{
    // 平面上的一个点
    QVector3D plane_point = in_lambda * in_plane_norm;

    // 三角形的三个顶点
    QVector3D v1 = in_mesh.v1;
    QVector3D v2 = in_mesh.v2;
    QVector3D v3 = in_mesh.v3;

    // 用于存储交点的临时变量
    QVector<QVector3D> intersections;

    // 在计算交点时，增加对边端点位于平面上的判断
    auto addIntersectionIfOnPlane = [&](const QVector3D& point) {
        if (std::abs(QVector3D::dotProduct(in_plane_norm, point - plane_point)) < 1e-6f) {
            intersections.push_back(point);
        }
    };

    // 检查三角形顶点是否在平面上
    addIntersectionIfOnPlane(v1);
    addIntersectionIfOnPlane(v2);
    addIntersectionIfOnPlane(v3);

    // 计算边 v1-v2 与平面的交点
    QVector3D edge1 = v2 - v1;
    float denom1 = QVector3D::dotProduct(in_plane_norm, edge1);
    if (std::fabs(denom1) > std::numeric_limits<float>::epsilon()) {
        float t1 = QVector3D::dotProduct(in_plane_norm, plane_point - v1) / denom1;
        if (t1 >= 0.0f && t1 <= 1.0f) {
            intersections.push_back(v1 + t1 * edge1);
        }
    }

    // 计算边 v2-v3 与平面的交点
    QVector3D edge2 = v3 - v2;
    float denom2 = QVector3D::dotProduct(in_plane_norm, edge2);
    if (std::fabs(denom2) > std::numeric_limits<float>::epsilon()) {
        float t2 = QVector3D::dotProduct(in_plane_norm, plane_point - v2) / denom2;
        if (t2 >= 0.0f && t2 <= 1.0f) {
            intersections.push_back(v2 + t2 * edge2);
        }
    }

    // 计算边 v3-v1 与平面的交点
    QVector3D edge3 = v1 - v3;
    float denom3 = QVector3D::dotProduct(in_plane_norm, edge3);
    if (std::fabs(denom3) > std::numeric_limits<float>::epsilon()) {
        float t3 = QVector3D::dotProduct(in_plane_norm, plane_point - v3) / denom3;
        if (t3 >= 0.0f && t3 <= 1.0f) {
            intersections.push_back(v3 + t3 * edge3);
        }
    }

    // 去重
    removeDuplicates(intersections);

    // 如果找到了两个交点并且不是同一个（来自交一个三角面片的顶点），则输出交线
    if (intersections.size() == 2)
    {
        out_edge.v1.pos = intersections[0];
        out_edge.v2.pos = intersections[1];
        out_edge.norm = in_plane_norm;
    }
    else
    {
        // 如果没有交点或交点不足两个，输出无效边
        out_edge.v1.pos = QVector3D();
        out_edge.v2.pos = QVector3D();
        out_edge.norm = QVector3D();
    }
}

QVector<Mesh> h0(const QVector<Mesh> &stl_model_meshes, const QVector3D &plane_norm, float lambda)
{
    QVector<Mesh> intersecting_meshes; // 存储与平面相交的三角面片

    // 计算平面上的点 P0
    QVector3D P0 = lambda * plane_norm;

    // 遍历所有三角面片
    for (const Mesh& mesh : stl_model_meshes) {
        // 获取三角面片的三个顶点
        QVector3D v1 = mesh.v1;
        QVector3D v2 = mesh.v2;
        QVector3D v3 = mesh.v3;

        // 计算每个顶点与平面的位置关系
        float d1 = QVector3D::dotProduct(plane_norm, v1 - P0);
        float d2 = QVector3D::dotProduct(plane_norm, v2 - P0);
        float d3 = QVector3D::dotProduct(plane_norm, v3 - P0);

        // 判断三角面片是否与平面相交
        if ((d1 * d2 < 0) || (d1 * d3 < 0) || (d2 * d3 < 0)) {
            // 如果三个顶点不全在平面的同一侧，则三角面片与平面相交
            intersecting_meshes.append(mesh);
        }
    }

    return intersecting_meshes;
}

void f1(const QVector<Mesh>& in_model_meshes, const QVector3D& in_plane_norm, float in_lambda, QMap<Vertex, QVector<Vertex>>& out_points_adjoin_map) {
    // 清空输出邻接关系
    out_points_adjoin_map.clear();

    // 遍历所有三角面片
    for (const Mesh& mesh : in_model_meshes) {
        Edge edge;
        g0(mesh, in_plane_norm, in_lambda, edge); // 计算交线

        // 如果交线有效（两个端点有效）
        if (edge.v1.pos != QVector3D() && edge.v2.pos != QVector3D() && edge.norm != QVector3D())
        {
            Vertex v1 = {edge.v1.pos, QVector3D(), QVector4D()}; // 创建顶点 v1
            Vertex v2 = {edge.v2.pos, QVector3D(), QVector4D()}; // 创建顶点 v2

            // 记录 v1 和 v2 的邻接关系

            out_points_adjoin_map[v1].push_back(v2); // v1 的邻接点是 v2
            out_points_adjoin_map[v2].push_back(v1); // v2 的邻接点是 v1
        }
    }
}

void h1(const QVector<Mesh>& in_model_meshes, const QVector3D& in_plane_norm, float in_lambda, QVector<Edge>& links)
{
    // 清空输出邻接关系
    links.clear();

    // 遍历所有三角面片
    for (const Mesh& mesh : in_model_meshes)
    {
        Edge edge;
        g0(mesh, in_plane_norm, in_lambda, edge); // 计算交线

        // 如果交线有效（两个端点有效）
        if (edge.v1.pos != QVector3D() && edge.v2.pos != QVector3D() && edge.norm != QVector3D())
            links.append(edge);
    }
}

// 判断两条边是否共享至少一个顶点
bool hasAtLeastOneSharedVertex(const Edge& a, const Edge& b) {
    return
        (a.v1 == b.v1) || (a.v1 == b.v2) ||
        (a.v2 == b.v1) || (a.v2 == b.v2);
}

void h2(const QVector<Edge>& links, QVector<QVector<Vertex>>& contours)
{
    // 用于记录已经处理过的边
    QSet<Edge> processedEdges;

    // 遍历所有边
    for (const Edge& edge : links)
    {
        // 如果这条边已经处理过，跳过
        if (processedEdges.contains(edge)) {
            continue;
        }

        // 创建一个新的轮廓
        QVector<Vertex> contour;

        // 使用广度优先搜索来找到所有相连的边
        QQueue<Edge> queue; // 使用 QQueue 实现 BFS
        queue.enqueue(edge);
        processedEdges.insert(edge);
        Vertex currentLeft = edge.v1, currentRight = edge.v2;
        while (!queue.isEmpty())
        {
            Edge currentEdge = queue.dequeue();
            // 将当前边的顶点加入轮廓
            if (!contour.contains(currentEdge.v1) && currentEdge.v2 == currentLeft)
            {
                contour.push_front(currentEdge.v1);
                currentLeft = currentEdge.v1;
            }
            else if (!contour.contains(currentEdge.v1) && currentEdge.v2 == currentRight) //
            {
                contour.push_back(currentEdge.v1);
                currentRight = currentEdge.v1;
            }
            if (!contour.contains(currentEdge.v2) && currentEdge.v1 == currentLeft)
            {
                contour.push_front(currentEdge.v2);
                currentLeft = currentEdge.v2;
            }
            else if (!contour.contains(currentEdge.v2) && currentEdge.v1 == currentRight)
            {
                contour.push_back(currentEdge.v2);
                currentRight = currentEdge.v2;
            }

            // 查找与当前边相连的边
            for (const Edge& nextEdge : links)
            {
                if (processedEdges.contains(nextEdge))
                    continue;

                if (hasAtLeastOneSharedVertex(currentEdge, nextEdge))
                {
                    queue.enqueue(nextEdge);
                    processedEdges.insert(nextEdge);
                }
            }
        }

        // 将找到的轮廓加入结果
        contours.append(contour/*.values().toVector()*/);
    }

    // 按轮廓大小排序
    std::sort(contours.begin(), contours.end(),
              [](const QVector<Vertex>& a, const QVector<Vertex>& b) {
                  return a.size() > b.size();
              });
}

void dfs(const Vertex& current, const QMap<Vertex, QVector<Vertex>>& in_points_adjoin_map, QSet<Vertex>& visited, QVector<Vertex>& contour)
{
    visited.insert(current); // 标记当前顶点为已访问
    contour.push_back(current); // 将当前顶点加入轮廓

    qDebug() << in_points_adjoin_map[current].size();
    if(in_points_adjoin_map[current].isEmpty())
        int i = 0;

    // 遍历当前顶点的邻接点
    for (const Vertex& neighbor : in_points_adjoin_map[current]) {
        if (!visited.contains(neighbor)) { // 如果邻接点未被访问
            dfs(neighbor, in_points_adjoin_map, visited, contour); // 递归访问邻接点
        }
    }
}

void bfs(const Vertex& start, const QMap<Vertex, QVector<Vertex>>& in_points_adjoin_map, QSet<Vertex>& visited, QVector<Vertex>& contour)
{
    QQueue<Vertex> queue; // 使用队列来存储待访问的顶点
    queue.enqueue(start); // 将起始顶点加入队列
    visited.insert(start); // 标记起始顶点为已访问
    contour.push_back(start); // 将起始顶点加入轮廓

    while (!queue.isEmpty())
    {
        Vertex current = queue.dequeue(); // 取出队列中的当前顶点

        // 遍历当前顶点的邻接点
        for (const Vertex& neighbor : in_points_adjoin_map.value(current))
        {
            if (!visited.contains(neighbor)) { // 如果邻接点未被访问
                visited.insert(neighbor); // 标记邻接点为已访问
                contour.push_back(neighbor); // 将邻接点加入轮廓
                queue.enqueue(neighbor); // 将邻接点加入队列
            }
        }
    }
}

void f2(const QMap<Vertex, QVector<Vertex>>& in_points_adjoin_map, QVector<QVector<Vertex>>& out_contours)
{
    // 清空输出轮廓
    out_contours.clear();

    // 用于标记已访问的点
    QSet<Vertex> visited;

    // 遍历所有邻接关系
    for (auto it = in_points_adjoin_map.begin(); it != in_points_adjoin_map.end(); ++it)
    {
        if (!visited.contains(it.key()))
        {
            const Vertex& current = it.key();   // 当前点
            QVector<Vertex> contour; // 新建一个轮廓
            bfs(current, in_points_adjoin_map, visited, contour); // 从当前顶点开始 BFS
            out_contours.push_back(contour); // 将找到的轮廓加入输出
        }
    }

    std::sort(out_contours.begin(), out_contours.end(),
              [](const QVector<Vertex>& a, const QVector<Vertex>& b) {
                  return a.size() > b.size();
              });
}

//void f2(const QMap<Vertex, QVector<Vertex>>& in_points_adjoin_map, QVector<QVector<Vertex>>& out_contours)
//{
//    // 清空输出轮廓
//    out_contours.clear();
//
//    // 临时存储未闭合的轮廓
//    QVector<QVector<Vertex>> pending_contours;
//
//    // 用于标记已访问的点
//    QSet<Vertex> visited;
//
//    // 遍历所有邻接数量为1的点（轮廓终点）
//    for (auto it = in_points_adjoin_map.begin(); it != in_points_adjoin_map.end(); ++it)
//    {
//        if (!visited.contains(it.key()))
//        { // 邻接数量为1且未访问
//            const Vertex& current = it.key();   // 当前点
//            QVector<Vertex> contour;  // 当前轮廓
//
//            // 如果当前顶点未被访问
//            if (!visited.contains(current))
//            {
//                QVector<Vertex> contour; // 新建一个轮廓
//                dfs(current, in_points_adjoin_map, visited, contour); // 从当前顶点开始 DFS
//                out_contours.push_back(contour); // 将找到的轮廓加入输出
//            }
//
//            // 将生成的轮廓加入输出
//            if (contour.size() >= 2) { // 至少需要两个点才能形成轮廓
//                out_contours.append(contour);
//            }
//        }
//    }
//
//    std::sort(out_contours.begin(), out_contours.end(),
//              [](const QVector<Vertex>& a, const QVector<Vertex>& b) {
//                  return a.size() > b.size();
//              });
//}

float distance(const Vertex& a, const Vertex& b) {
    return (a.pos - b.pos).length();
}

void g3(const QVector<Vertex>& in_seg1, const QVector<Vertex>& in_seg2, float in_adjoin_dis_epsilon, QVector<Vertex>& out_combined_seg)
{
    out_combined_seg.clear(); // 清空输出

    if (in_seg1.isEmpty() || in_seg2.isEmpty()) {
        return; // 如果任一输入为空，直接返回
    }

    // 提取首尾顶点
    Vertex seg1Start = in_seg1.first();
    Vertex seg1End = in_seg1.last();
    Vertex seg2Start = in_seg2.first();
    Vertex seg2End = in_seg2.last();

    // 检查四种可能的拼接情况
    if (distance(seg1End, seg2Start) < in_adjoin_dis_epsilon) {
        // seg1尾 -> seg2头
        out_combined_seg.append(in_seg1);
        out_combined_seg.append(in_seg2);
    } else if (distance(seg1End, seg2End) < in_adjoin_dis_epsilon) {
        // seg1尾 -> seg2尾（需反转seg2）
        QVector<Vertex> reversedSeg2 = in_seg2;
        std::reverse(reversedSeg2.begin(), reversedSeg2.end());
        out_combined_seg.append(in_seg1);
        out_combined_seg.append(reversedSeg2);
    } else if (distance(seg1Start, seg2Start) < in_adjoin_dis_epsilon) {
        // seg1头 -> seg2头（需反转seg2）
        QVector<Vertex> reversedSeg2 = in_seg2;
        std::reverse(reversedSeg2.begin(), reversedSeg2.end());
        out_combined_seg.append(reversedSeg2);
        out_combined_seg.append(in_seg1);
    } else if (distance(seg1Start, seg2End) < in_adjoin_dis_epsilon) {
        // seg1头 -> seg2尾
        out_combined_seg.append(in_seg2);
        out_combined_seg.append(in_seg1);
    }

    // 如果没有满足条件的拼接，out_combined_seg 保持为空
}

void f3(QVector<QVector<Vertex>>& mod_contours, float in_adjoin_dis_epsilon) {
    bool merged;
    do {
        merged = false;

        for (int i = 0; i < mod_contours.size(); ++i) {
            if (mod_contours[i].isEmpty()) continue;

            bool continueMerging = true;
            while (continueMerging) {
                continueMerging = false;
                for (int j = mod_contours.size() - 1; j >= 0; --j) {
                    if (i == j || mod_contours[j].isEmpty()) continue;

                    QVector<Vertex> combinedSeg;
                    g3(mod_contours[i], mod_contours[j], in_adjoin_dis_epsilon, combinedSeg);

                    if (!combinedSeg.isEmpty()) {
                        mod_contours[i] = combinedSeg;
                        mod_contours.removeAt(j);
                        merged = true;
                        continueMerging = true;

                        // 调整索引，确保 i 不越界
                        if (i > 0) {
                            i--;
                        }
                        break;
                    }
                }
            }
        }
    } while (merged);
}

QVector3D computeContourCentroid(const QVector<Vertex>& contour)
{
    QVector3D centroid = contour.first().pos;
    for(int i = 1; i < contour.size(); ++i)
    {
        centroid += contour[i].pos;
        centroid /= 2;
    }
    return centroid;
}

// 计算三维空间中切平面上的多边形面积
float computeContourArea(const QVector<Vertex>& contour)
{
    if (contour.size() < 3) {
        return 0.0f; // 无法形成多边形
    }

    float area = 0.0f;
    int n = contour.size();

    // 选择一个参考点（第一个点）
    const QVector3D& p0 = contour[0].pos;

    // 遍历多边形的边
    for (int i = 1; i < n - 1; ++i) {
        const QVector3D& p1 = contour[i].pos;
        const QVector3D& p2 = contour[i + 1].pos;

        // 计算两个向量（p1 - p0 和 p2 - p0）
        QVector3D v1 = p1 - p0;
        QVector3D v2 = p2 - p0;

        // 计算叉积
        QVector3D cross = QVector3D::crossProduct(v1, v2);

        // 累加叉积的长度（三角形面积的两倍）
        area += cross.length();
    }

    // 面积是叉积长度的一半
    area /= 2.0f;

    // 确保面积为正
    return std::abs(area);
}

// 合并两个轮廓
QVector<Vertex> combineContours(const QVector<Vertex>& contour_a, const QVector<Vertex>& contour_b)
{
    QVector<Vertex> combined_contour = contour_a;
    combined_contour += contour_b;
    return combined_contour;
}

void g4(QVector<Vertex> &contour_a, QVector<Vertex> &contour_b, float epsilon, QVector<Vertex> &out_combined_contour)
{
    // 计算原始轮廓的面积
    float area_a = computeContourArea(contour_a);
    float area_b = computeContourArea(contour_b);

    // 合并两个轮廓
    QVector<Vertex> combined_contour = combineContours(contour_a, contour_b);

    // 计算合并后轮廓的面积
    float area_combined = computeContourArea(combined_contour);

    // 计算面积变化
    float area_change_a = std::fabs(area_combined - area_a);
    float area_change_b = std::fabs(area_combined - area_b);

    // 判断变化是否在容差范围内
    if (area_change_a <= epsilon || area_change_b <= epsilon) {
        // 变化较小，认为是边缘插值点，合并轮廓
        out_combined_contour = combined_contour;
    } else {
        // 变化较大，认为是非边缘点，不合并
        out_combined_contour.clear();
    }
}

void f4(const QVector<QVector<Vertex>>& contours, float epsilon, QVector<QVector<Vertex>>& out_combined_contours)
{
    QVector<QVector<Vertex>> _contours(contours);

    // 清空输出
    out_combined_contours.clear();

    // 用于标记已经合并的轮廓
    QSet<int> merged_indices;

    // 遍历所有轮廓
    for (int i = 0; i < _contours.size(); ++i) {
        if (merged_indices.contains(i)) {
            // 如果当前轮廓已经被合并，跳过
            continue;
        }

        // 初始化合并后的轮廓为当前轮廓
        QVector<Vertex> combined_contour = _contours[i];

        // 尝试与其他轮廓合并
        for (int j = i + 1; j < _contours.size(); ++j)
        {
            if (merged_indices.contains(j)) {
                // 如果目标轮廓已经被合并，跳过
                continue;
            }

            // 调用 g4 函数判断是否应该合并
            QVector<Vertex> temp_combined_contour;
            g4(combined_contour, _contours[j], epsilon, temp_combined_contour);

            if (!temp_combined_contour.isEmpty()) {
                // 如果合并成功，更新 combined_contour 并标记目标轮廓为已合并
                combined_contour = temp_combined_contour;
                merged_indices.insert(j);
            }
        }

        // 将合并后的轮廓加入输出
        out_combined_contours.append(combined_contour);
    }
}
